//------------------------------------------------------------------------------
//  <copyright file="TestCollisionBasics.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Test.Robotics.Collision
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Collision;
    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Test class for basic collision checking methods
    /// </summary>
    [TestClass]
    public class TestCollisionBasics
    {
        /// <summary>
        /// Tests computing distances between a triangle and a sphere
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestDistanceBetweenTriangleAndSphere()
        {
            // create a triangle in the environment
            TriangleElement triangle = new TriangleElement(new Vector3(1, 1, 0), new Vector3(1, 2, 0), new Vector3(2, 2, 0));

            SphereElement sphere1 = new SphereElement(new Vector3(0, 1.5, 2), 1);
            double distance1 = triangle.DistanceFrom(sphere1);
            Assert.AreEqual(distance1, Math.Sqrt(5) - sphere1.Radius);

            SphereElement sphere2 = new SphereElement(new Vector3(0, 3, 2), 1);
            double distance2 = triangle.DistanceFrom(sphere2);
            Assert.AreEqual(distance2, Math.Sqrt(6) - sphere2.Radius);

            SphereElement sphere3 = new SphereElement(new Vector3(1.5, 3, 2), 1);
            double distance3 = triangle.DistanceFrom(sphere3);
            Assert.AreEqual(distance3, Math.Sqrt(5) - sphere3.Radius);

            SphereElement sphere4 = new SphereElement(new Vector3(4, 3, 2), 1);
            double distance4 = triangle.DistanceFrom(sphere4);
            Assert.AreEqual(distance4, 3 - sphere4.Radius);

            SphereElement sphere5 = new SphereElement(new Vector3(2, 1, 2), 1);
            double distance5 = triangle.DistanceFrom(sphere5);
            Assert.AreEqual(distance5, Math.Sqrt(4.5) - sphere5.Radius);

            SphereElement sphere6 = new SphereElement(new Vector3(0.5, 0, 2), 1);
            double distance6 = triangle.DistanceFrom(sphere6);
            Assert.AreEqual(distance6, Math.Sqrt(5.25) - sphere6.Radius);

            SphereElement sphere7 = new SphereElement(new Vector3(1.5, 1.75, 2), 1);
            double distance7 = triangle.DistanceFrom(sphere7);
            Assert.AreEqual(distance7, 2 - sphere7.Radius);
        }

        /// <summary>
        /// Tests computing the distances between two spheres
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestDistanceBetweenSpheres()
        {
            SphereElement sphere1 = new SphereElement(new Vector3(0, 15, 3), 1);
            SphereElement sphere2 = new SphereElement(new Vector3(0, 15, 2), 1);
            double distance1 = sphere1.DistanceFrom(sphere2);
            Assert.AreEqual(distance1, 1 - sphere1.Radius - sphere2.Radius);

            SphereElement sphere3 = new SphereElement(new Vector3(0, 15, 20), 1);
            double distance2 = sphere1.DistanceFrom(sphere3);
            Assert.AreEqual(distance2, 17 - sphere1.Radius - sphere2.Radius);
        }

        /// <summary>
        /// Tests computing distances between two objects which consist of basic elements
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestDistanceBetweenGeometryElementGroups()
        {
            // define eight vertices of a rectangular cube
            Vector3 v1 = new Vector3(0, 0, 0);
            Vector3 v2 = new Vector3(0, 0, 1);
            Vector3 v3 = new Vector3(3, 0, 1);
            Vector3 v4 = new Vector3(3, 0, 0);
            Vector3 v5 = new Vector3(0, 2, 0);
            Vector3 v6 = new Vector3(0, 2, 1);
            Vector3 v7 = new Vector3(3, 2, 1);
            Vector3 v8 = new Vector3(3, 2, 0);

            List<GeometryElement3D> triangles = new List<GeometryElement3D>();
            triangles.Add(new TriangleElement(v1, v2, v3));
            triangles.Add(new TriangleElement(v1, v3, v4));

            triangles.Add(new TriangleElement(v4, v3, v7));
            triangles.Add(new TriangleElement(v4, v7, v8));

            triangles.Add(new TriangleElement(v8, v7, v6));
            triangles.Add(new TriangleElement(v8, v6, v5));

            triangles.Add(new TriangleElement(v5, v6, v2));
            triangles.Add(new TriangleElement(v5, v2, v1));

            triangles.Add(new TriangleElement(v2, v6, v7));
            triangles.Add(new TriangleElement(v2, v7, v3));

            triangles.Add(new TriangleElement(v1, v4, v8));
            triangles.Add(new TriangleElement(v1, v8, v5));
            GeometryElementGroup3D cube1 = new GeometryElementGroup3D(triangles);

            // represent an arm with three spheres
            SphereElement sphere1 = new SphereElement(new Vector3(10, 0, 0), 1);
            SphereElement sphere2 = new SphereElement(new Vector3(12, 0, 0), 1);
            SphereElement sphere3 = new SphereElement(new Vector3(14, 0, 0), 1);
            List<GeometryElement3D> spheres1 = new List<GeometryElement3D>();
            spheres1.Add(sphere1);
            spheres1.Add(sphere2);
            spheres1.Add(sphere3);
            GeometryElementGroup3D arm1 = new GeometryElementGroup3D(spheres1);

            SphereElement sphere4 = new SphereElement(new Vector3(-10, 0, 0), 1);
            SphereElement sphere5 = new SphereElement(new Vector3(-12, 0, 0), 1);
            SphereElement sphere6 = new SphereElement(new Vector3(-14, 0, 0), 1);
            List<GeometryElement3D> spheres2 = new List<GeometryElement3D>();
            spheres2.Add(sphere4);
            spheres2.Add(sphere5);
            spheres2.Add(sphere6);
            GeometryElementGroup3D arm2 = new GeometryElementGroup3D(spheres2);

            // check collision between two objects using brute force
            double distance = arm1.DistanceFrom(cube1);
            Assert.AreEqual(distance, 6);

            CollisionCheckingBruteForce checker = new CollisionCheckingBruteForce();
            List<GeometryElementGroup3D> arms = new List<GeometryElementGroup3D>();
            arms.Add(arm1);
            arms.Add(arm2);
            List<GeometryElementGroup3D> cubes = new List<GeometryElementGroup3D>();
            cubes.Add(cube1);
            double[][] distances;

            double collisionDistance = checker.CheckCollision(arms, cubes, out distances);

            Assert.AreEqual(distances.Length, 2);
            Assert.AreEqual(distances[0].Length, 1);
            Assert.AreEqual(collisionDistance, 6);
            Assert.AreEqual(distances[0][0], 6);
            Assert.AreEqual(distances[1][0], 9);

            // a case where collision happens
            SphereElement sphere7 = new SphereElement(new Vector3(4, 0, 0), 2);
            SphereElement sphere8 = new SphereElement(new Vector3(7, 0, 0), 2);
            SphereElement sphere9 = new SphereElement(new Vector3(10, 0, 0), 2);
            List<GeometryElement3D> spheres3 = new List<GeometryElement3D>();
            spheres3.Add(sphere7);
            spheres3.Add(sphere8);
            spheres3.Add(sphere9);
            GeometryElementGroup3D arm3 = new GeometryElementGroup3D(spheres3);
            arms.Add(arm3);
            collisionDistance = checker.CheckCollision(arms, cubes, out distances);
            Assert.AreEqual(distances[2][0], -1);
        }
    }
}
